/*
 * controllerEmotion.cpp
 *
 *  Created on: Nov 27, 2012
 *      Author: michiel
 */

#include "PS3Controller/controller.h"

void Controller::init() {
	emo_pub_ = nh_.advertise<comm::emotion>(Topic::EMO_STATE, 1);
}

void Controller::spin() {
	ros::Rate loop_rate(30);

	while (ros::ok()) {
		comm::emotion message;

		if (crossPressed_) {
			ROS_INFO("Expressing HAPPY!");
			message.emotion = EMOTION_HAPPY;
			message.state		= true;
		} else if (trianglePressed_) {
			ROS_INFO("Expressing SAD!");
			message.emotion	= EMOTION_SAD;
			message.state		= true;
		} else if (squarePressed_) {
			ROS_INFO("Expressing BORED!");
			message.emotion	= EMOTION_BORED;
			message.state		= true;
		} else {
			ROS_INFO("Expressing NOTHING!");
			message.state		= false;
		}

		emo_pub_.publish(message);
		ros::spinOnce();
		loop_rate.sleep();
	}

}

void Controller::controllerCallback(const sensor_msgs::Joy::ConstPtr &message) {
	buttons_	= message->buttons;

	buttonCross_		=	buttons_[PS3_BUTTON_ACTION_CROSS];
	buttonTriangle_	=	buttons_[PS3_BUTTON_ACTION_TRIANGLE];
	buttonSquare_		=	buttons_[PS3_BUTTON_ACTION_SQUARE];

	if (!crossPressed_ && buttonCross_ == 1) {
		crossPressed_ = true;
	} else if (crossPressed_ && buttonCross_ == 0) {
		crossPressed_ = false;
	}

	if (!trianglePressed_ && buttonTriangle_ == 1) {
		trianglePressed_ = true;
	} else if (trianglePressed_ && buttonTriangle_ == 0) {
		trianglePressed_ = false;
	}

	if (!squarePressed_ && buttonSquare_ == 1) {
		squarePressed_ = true;
	} else if (squarePressed_ && buttonSquare_ == 0) {
		squarePressed_ = false;
	}
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "ControllerEmotionPS3");

	Controller controller;
	controller.init();
	controller.spin();
}



